/****************************************************
 * 用来模拟规划和定位模块，给控制模块发送路径规划和车辆定位的信息
 * @author:HuYunhao
 * @date:2021/05/24*/
#ifndef SRC_PATH_PLAN_H
#define SRC_PATH_PLAN_H
#include "my_functions.h"
#include "path_plan.h"
#include "eigen/Eigen/Dense"
#include "polyfit.h"        //多项式拟合
using namespace std;
using Eigen::VectorXd;

namespace pathPlan_NS {
class State {
    /****************************************
     * 用来初始化和更新车辆状态的类
     * 车辆状态向量state = [x,y,yaw,v,cte,ephi]
     * cte是横向偏差[m]，ephi是航向偏差[deg]/
     * */
public:
    State(double x, double y, double phi, double v);
    void calError(const VectorXd& coeffs);
public:
    double x_;          //全局坐标系下，车辆质心的x坐标
    double y_;
    double phi_;        //车辆的yaw angle[deg]
    double v_;          //[m/s]
    double cte_;        //cross-track error,横向偏差[m]
    double ephi_;       //heading error,航向偏差[deg]
    VectorXd state_;    //state = [x, y, yaw, v, cte, ephi]
};
class DesiredPath{
    /***************************************
     * 接收来自规划的路径点，并且使用多项式拟合的方式拟合路径点
     * */
public:
    DesiredPath();                                  //从路径txt文件中读取路径点.
public:
    std::vector<double> waypoints_x_;       //从规划读取的x坐标
    std::vector<double> waypoints_y_;       //从规划读取的y坐标
    VectorXd coeffs_;
};

}       //namespace pathPlan_NS

#endif //SRC_PATH_PLAN_H
